#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# Copyright (C) 2023 reinovo, Inc. All Rights Reserved 
#
# @Time    : 2024/1/12 下午2:00
# @Author  : hmm
# @Email   : liuyuhang0531@foxmail.com
# @File    : test2.py
# @Software: PyCharm

from arm_control import uarm_control
import cv2
import numpy as np

mtx = np.load('../data/camera_param.npz')['mtx']
with np.load('../data/eyehand_Matrix.npz') as X:
    R, t = [X[i] for i in ('R', 't')]

arm = uarm_control.Arm_controller(mtx,R,t)

print("----------------------")
wh = arm.make_warehouse([150,80,10],[2,2,2],[30,30,10])
arm.move([200,0,150])

cap = cv2.VideoCapture(4);
while cap.isOpened():
    _, frame = cap.read()
    cv2.imshow("frame",frame)
    if cv2.waitKey(10)==27:
        break
cv2.destroyAllWindows()
cap.release()

tags = arm.tf.get_img2tag(frame, 20)
i = 0
for tag in tags:
    print(tag[3])
    pos = arm.tag_point(tag)
    arm.gate_move(pos,wh[i],50,offset_angle=-tag[3][2])
    arm.move([200, 0, 150])
    i+=1;
    if i >= len(wh):
        break


